﻿#pragma once
#include <QByteArray>
#include <vector>
#include "common_data.h"

class  CommonFun
{
public:
	CommonFun();


	// 四元数转欧拉角
	static Position Quat2Eul(const PositionQuaternion& input);
	// 欧拉角转四元数
	static PositionQuaternion Eul2Quat(const Position& input);

	/********************************************************************
	* 功能：坐标系转换
	* 参数：a： A在B坐标下的坐标，b:B在其它坐标系的坐标
	* 返回值：A在其它坐标系下的坐标
	***************************************************************************/
	static Position TransAToBCoordinateSystem(const Position& a, const Position& b);
	static Position TransAToBCoordinateSystem(const Position& a, const PositionQuaternion& b);
	static Position TransAToBCoordinateSystem(const PositionQuaternion& a, const Position& b);
	static Position TransAToBCoordinateSystem(const PositionQuaternion& a, const PositionQuaternion& b);

	/*************************************************************
	* 已知A在B坐标系下的坐标，求B在A坐标下的坐标
	* 参数： A在B坐标系的坐标
	* 返回值：B在A坐标系下的坐标
	*****************************************************************/
	static PositionQuaternion FromAToBQuat(const Position& a);
	static PositionQuaternion FromAToBQuat(const PositionQuaternion& a);
	static Position FromAToBEuler(const Position& a);
	static Position FromAToBEuler(const PositionQuaternion& a);

	static void Sleep(int msec);

	static QByteArray ToBin(int val, int size);
	static QByteArray ToBin(float vale, int size);

	static unsigned int FromBinToInt(const QByteArray& d);
	static float FromBinToFloat(const QByteArray& d);
	static QString FromBinToString(const QByteArray& d);

	/****************************************************************************
 * 功能: 对容器进行排序，输出排序后的索引顺序
 * 传参: std::vector<T> input, order: 排序方式 0：从小到大，1：从大到小
 * 返回值:
 ****************************************************************************/
	template <typename T>
	static std::vector<int> sort_index(const std::vector<T>& input, int order)
	{
		// 生成0-n的容器
		std::vector<int> out(input.size());
		std::iota(out.begin(), out.end(), 0);	// 向容器中填充0-n 的数字
		if (order == 0) {
			std::sort(out.begin(), out.end(), [&input](size_t i1, size_t i2) {
				return input[i1] < input[i2];
				});
		} else {
			std::sort(out.begin(), out.end(), [&input](size_t i1, size_t i2) {
				return input[i1] < input[i2];
				});
		}
		return out;
	}
};
